/*!
  @file image_publisher.cc
  @brief Brief description
  @author Kyohei Otsu <kyohei@kth.se>
  @date 2013
*/

#include <fstream>

#include <ros/ros.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/CameraInfo.h>

using namespace std;

namespace vo
{

//! class description 
/*! This is a detailed description of the class */
class ImagePublisher {
 private:
  //! ROS node handler
  ros::NodeHandle nh_;
  //! ROS node handler in the private namespace
  ros::NodeHandle private_nh_;
  //!
  image_transport::ImageTransport it_;
  //! 
  image_transport::CameraPublisher img_pub_;
  //! 
  image_transport::CameraPublisher imgL_pub_;
  //! 
  image_transport::CameraPublisher imgR_pub_;

  //! image file name
  std::string filename_;
  //! image list name
  std::string imgL_list_;
  //! image list name
  std::string imgR_list_;

  //! simuration rate
  double sim_rate_;


 protected:

 public:
  //! Constructor
  ImagePublisher(ros::NodeHandle &nh) : nh_(nh), private_nh_("~"), it_(nh)
  {
    // initializing pub/sub, server
    img_pub_ = it_.advertiseCamera(nh_.resolveName("image_out"), 1);
    imgL_pub_ = it_.advertiseCamera(nh_.resolveName("imageL_out"), 1);
    imgR_pub_ = it_.advertiseCamera(nh_.resolveName("imageR_out"), 1);

    // initializing optional parameters
    // e.g.) private_nh_.param<TYPE>("PARAM_NAME", PARAM_VAR, DEFAULT_VALUE);
    private_nh_.param<std::string>("filename", filename_, "pattern.png");
    private_nh_.param<std::string>("imageL_list", imgL_list_, "imgL");
    private_nh_.param<std::string>("imageR_list", imgR_list_, "imgL"); 
    private_nh_.param<double>("sim_rate", sim_rate_, 10.0);
  }

  //! Destructor
  ~ImagePublisher()
  {
  }

  //!
  void publish_one_img()
  {
    ROS_INFO_STREAM("opening " << filename_);
    cv::Mat image = cv::imread(filename_, 1);
    if(!image.data)
    {
      ROS_ERROR("failed opening image file");
      ROS_BREAK();
    }

    cv_bridge::CvImage::Ptr cv_ptr(new cv_bridge::CvImage);
    cv_ptr->header.stamp = ros::Time::now();
    cv_ptr->header.frame_id = "camera_frame";
    cv_ptr->encoding = "bgr8";
    cv_ptr->image = image;

    sensor_msgs::CameraInfo::Ptr info_ptr(new sensor_msgs::CameraInfo);
    info_ptr->header.stamp = cv_ptr->header.stamp;
    info_ptr->header.frame_id = cv_ptr->header.frame_id;
    info_ptr->width = cv_ptr->image.cols;
    info_ptr->height = cv_ptr->image.rows;

    img_pub_.publish(cv_ptr->toImageMsg(), info_ptr);
  }

  //!
  void publish_stereo_imgs()
  {
    ifstream ifsL(imgL_list_.c_str(), ifstream::in); 
    ifstream ifsR(imgR_list_.c_str(), ifstream::in);
    if(!ifsL.is_open() || !ifsR.is_open())
    {
      ROS_ERROR("File open");
      ROS_BREAK();
    }

    cv_bridge::CvImage::Ptr cv_ptrL(new cv_bridge::CvImage);
    cv_bridge::CvImage::Ptr cv_ptrR(new cv_bridge::CvImage);
    cv_ptrL->header.frame_id = "camera_frameL";
    cv_ptrR->header.frame_id = "camera_frameR";
    cv_ptrL->header.stamp = cv_ptrR->header.stamp = ros::Time::now();
    cv_ptrL->encoding = cv_ptrR->encoding = "bgr8";

    sensor_msgs::CameraInfo::Ptr info_ptrL(new sensor_msgs::CameraInfo);
    sensor_msgs::CameraInfo::Ptr info_ptrR(new sensor_msgs::CameraInfo);
    info_ptrL->header.stamp = info_ptrR->header.stamp = cv_ptrL->header.stamp;
    info_ptrL->header.frame_id = cv_ptrL->header.frame_id;
    info_ptrR->header.frame_id = cv_ptrR->header.frame_id;
    info_ptrL->width  = info_ptrR->width  = cv_ptrL->image.cols;
    info_ptrL->height = info_ptrR->height = cv_ptrL->image.rows;

    string lineL, lineR;
    ros::Rate r(sim_rate_);
    while(getline(ifsL, lineL) && getline(ifsR, lineR))
    {
      cv_ptrL->image = cv::imread(lineL, 1);
      cv_ptrR->image = cv::imread(lineR, 1);
      imgL_pub_.publish(cv_ptrL->toImageMsg(), info_ptrL);
      imgR_pub_.publish(cv_ptrR->toImageMsg(), info_ptrR);
      ros::spinOnce();
      r.sleep();
    }
  }
};

} // end of namespace

int main(int ac, char **av)
{
  ros::init(ac, av, "ImagePublisher");
  ros::NodeHandle nh;
  vo::ImagePublisher handler(nh);
  ros::Duration(3.0).sleep();
  //handler.publish_one_img();
  handler.publish_stereo_imgs();
  ros::spin();
  return 0;
}

